#include "mpu6050_eswin.h"
#include "log.h"
#include "math.h"

// 超时时间 (毫秒)
#define MPU6050_TIMEOUT_MS 50

// 加速度计量程对应的比例因子 (LSB/g)
static const float accel_scales[] = {
    16384.0f, // ±2g
    8192.0f,  // ±4g
    4096.0f,  // ±8g
    2048.0f   // ±16g
};

// 陀螺仪量程对应的比例因子 (LSB/°/s)
static const float gyro_scales[] = {
    131.0f, // ±250°/s
    65.5f,  // ±500°/s
    32.8f,  // ±1000°/s
    16.4f   // ±2000°/s
};

// 温度传感器比例因子 (LSB/°C)
#define TEMP_SCALE  340.0f
#define TEMP_OFFSET 36.53f

/**
 * @brief 从寄存器读取一个字节
 */
static status_t MPU6050_ReadRegister(mpu6050_handle_t *handle, uint8_t reg, uint8_t *data)
{
    return I2C_DRV_MasterSendDataBlocking(handle->i2cInstance, &reg, 1, false, MPU6050_TIMEOUT_MS) |
           I2C_DRV_MasterReceiveDataBlocking(handle->i2cInstance, data, 1, true, MPU6050_TIMEOUT_MS);
}

/**
 * @brief 向寄存器写入一个字节
 */
static status_t MPU6050_WriteRegister(mpu6050_handle_t *handle, uint8_t reg, uint8_t data)
{
    uint8_t tx_data[2] = {reg, data};
    return I2C_DRV_MasterSendDataBlocking(handle->i2cInstance, tx_data, sizeof(tx_data), true, MPU6050_TIMEOUT_MS);
}

/**
 * @brief 从寄存器读取多个字节
 */
static status_t MPU6050_ReadRegisters(mpu6050_handle_t *handle, uint8_t reg, uint8_t *data, uint8_t length)
{
    return I2C_DRV_MasterSendDataBlocking(handle->i2cInstance, &reg, 1, false, MPU6050_TIMEOUT_MS) |
           I2C_DRV_MasterReceiveDataBlocking(handle->i2cInstance, data, length, true, MPU6050_TIMEOUT_MS);
}

status_t MPU6050_Init(mpu6050_handle_t *handle, uint8_t i2cInstance, i2c_master_state_t *i2cState)
{
    if (handle == NULL) {
        return STATUS_ERROR;
    }

    // 初始化句柄
    memset(handle, 0, sizeof(mpu6050_handle_t));
    handle->i2cInstance = i2cInstance;
    handle->i2cState    = i2cState;

    // 检查设备ID
    uint8_t device_id = 0;
    status_t status   = MPU6050_ReadRegister(handle, MPU6050_REG_WHO_AM_I, &device_id);

    if (status != STATUS_SUCCESS) {
        log_err("MPU6050 I2C communication error\r\n");
        return status;
    }

    // 设备ID应为0x68 (MPU6050) 或 0x71 (MPU6500)
//    if (device_id != 0x68 && device_id != 0x71) {
//        log_err("MPU6050 not found. Device ID: 0x%02X\r\n", device_id);
//        return STATUS_ERROR;
//    }

    // 唤醒设备
    status = MPU6050_WriteRegister(handle, MPU6050_REG_PWR_MGMT_1, 0x00);
    OS_DelayMs(100); // 等待启动

    // 默认配置
    if (MPU6050_Config(handle, ACCEL_RANGE_2G, GYRO_RANGE_250DPS, DLPF_BW_94HZ) != STATUS_SUCCESS) {
        log_err("MPU6050 configuration failed\r\n");
        return STATUS_ERROR;
    }

    log_info("MPU6050 initialized successfully\r\n");
    return STATUS_SUCCESS;
}

status_t MPU6050_Config(mpu6050_handle_t *handle,
                        mpu6050_accel_range_t accel_range,
                        mpu6050_gyro_range_t gyro_range,
                        mpu6050_dlpf_t dlpf)
{
    // 设置采样率分频器 (1kHz / (1 + divider))
    if (MPU6050_WriteRegister(handle, MPU6050_REG_SMPLRT_DIV, 0x00) != STATUS_SUCCESS) {
        return STATUS_ERROR;
    }

    // 设置数字低通滤波器
    if (MPU6050_WriteRegister(handle, MPU6050_REG_CONFIG, dlpf) != STATUS_SUCCESS) {
        return STATUS_ERROR;
    }

    // 设置陀螺仪量程
    if (MPU6050_WriteRegister(handle, MPU6050_REG_GYRO_CONFIG, gyro_range << 3) != STATUS_SUCCESS) {
        return STATUS_ERROR;
    }

    // 设置加速度计量程
    if (MPU6050_WriteRegister(handle, MPU6050_REG_ACCEL_CONFIG, accel_range << 3) != STATUS_SUCCESS) {
        return STATUS_SUCCESS;
    }

    // 禁用中断
    if (MPU6050_WriteRegister(handle, MPU6050_REG_INT_ENABLE, 0x00) != STATUS_SUCCESS) {
        return STATUS_ERROR;
    }

    // 保存配置
    handle->accel_range = accel_range;
    handle->gyro_range  = gyro_range;
    handle->accel_scale = accel_scales[accel_range];
    handle->gyro_scale  = gyro_scales[gyro_range];

    return STATUS_SUCCESS;
}

status_t MPU6050_ReadRawData(mpu6050_handle_t *handle, mpu6050_raw_data_t *data)
{
    uint8_t buffer[14];

    // 读取14个字节的传感器数据 (加速度计、温度、陀螺仪)
    status_t status = MPU6050_ReadRegisters(handle, MPU6050_REG_ACCEL_XOUT_H, buffer, sizeof(buffer));
    if (status != STATUS_SUCCESS) {
        return status;
    }

    // 解析加速度计数据
    data->accel_x = (int16_t)((buffer[0] << 8) | buffer[1]);
    data->accel_y = (int16_t)((buffer[2] << 8) | buffer[3]);
    data->accel_z = (int16_t)((buffer[4] << 8) | buffer[5]);

    // 解析温度数据
    data->temp = (int16_t)((buffer[6] << 8) | buffer[7]);

    // 解析陀螺仪数据
    data->gyro_x = (int16_t)((buffer[8] << 8) | buffer[9]);
    data->gyro_y = (int16_t)((buffer[10] << 8) | buffer[11]);
    data->gyro_z = (int16_t)((buffer[12] << 8) | buffer[13]);

    // 应用校准偏移
    if (handle->calib.accel_x_offset != 0 ||
        handle->calib.accel_y_offset != 0 ||
        handle->calib.accel_z_offset != 0) {
        data->accel_x -= handle->calib.accel_x_offset;
        data->accel_y -= handle->calib.accel_y_offset;
        data->accel_z -= handle->calib.accel_z_offset;
    }

    if (handle->calib.gyro_x_offset != 0 ||
        handle->calib.gyro_y_offset != 0 ||
        handle->calib.gyro_z_offset != 0) {
        data->gyro_x -= handle->calib.gyro_x_offset;
        data->gyro_y -= handle->calib.gyro_y_offset;
        data->gyro_z -= handle->calib.gyro_z_offset;
    }

    return STATUS_SUCCESS;
}

char int2float[16];

status_t MPU6050_ReadAccel(mpu6050_handle_t *handle, float *accel_x, float *accel_y, float *accel_z)
{
    mpu6050_raw_data_t raw_data;
    status_t status = MPU6050_ReadRawData(handle, &raw_data);
    if (status != STATUS_SUCCESS) {
        return status;
    }
    sprintf(int2float, "%d", raw_data.accel_x);
    *accel_x = atof(int2float) / handle->accel_scale;
    sprintf(int2float, "%d", raw_data.accel_y);
    *accel_y = atof(int2float) / handle->accel_scale;
    sprintf(int2float, "%d", raw_data.accel_z);
    *accel_z = atof(int2float) / handle->accel_scale +1;

    return STATUS_SUCCESS;
}

status_t MPU6050_ReadGyro(mpu6050_handle_t *handle, float *gyro_x, float *gyro_y, float *gyro_z)
{
    mpu6050_raw_data_t raw_data;
    status_t status = MPU6050_ReadRawData(handle, &raw_data);
    if (status != STATUS_SUCCESS) {
        return status;
    }
    sprintf(int2float, "%d", raw_data.gyro_x);
    *gyro_x = atof(int2float) / handle->gyro_scale;
    sprintf(int2float, "%d", raw_data.gyro_y);
    *gyro_y = atof(int2float) / handle->gyro_scale;
    sprintf(int2float, "%d", raw_data.gyro_z);
    *gyro_z = atof(int2float) / handle->gyro_scale;

    return STATUS_SUCCESS;
}

status_t MPU6050_ReadTemperature(mpu6050_handle_t *handle, float *temperature)
{
    mpu6050_raw_data_t raw_data;
    status_t status = MPU6050_ReadRawData(handle, &raw_data);
    if (status != STATUS_SUCCESS) {
        return status;
    }
    sprintf(int2float, "%d.0", raw_data.temp);
    *temperature = (atof(int2float) / TEMP_SCALE) + TEMP_OFFSET;
    return STATUS_SUCCESS;
}

status_t MPU6050_Calibrate(mpu6050_handle_t *handle, uint16_t samples)
{
    if (samples == 0) {
        return STATUS_ERROR;
    }

    log_info("Starting MPU6050 calibration. Keep sensor stationary...\r\n");

    // 初始化累加器
    int32_t accel_x_sum = 0, accel_y_sum = 0, accel_z_sum = 0;
    int32_t gyro_x_sum = 0, gyro_y_sum = 0, gyro_z_sum = 0;

    // 临时存储原始数据
    mpu6050_raw_data_t raw_data;

    // 采集样本
    for (uint16_t i = 0; i < samples; i++) {
        if (MPU6050_ReadRawData(handle, &raw_data) != STATUS_SUCCESS) {
            log_err("Calibration sample %d failed\r\n", i);
            return STATUS_ERROR;
        }

        accel_x_sum += raw_data.accel_x;
        accel_y_sum += raw_data.accel_y;
        accel_z_sum += raw_data.accel_z;

        gyro_x_sum += raw_data.gyro_x;
        gyro_y_sum += raw_data.gyro_y;
        gyro_z_sum += raw_data.gyro_z;

        OS_DelayMs(10); // 采样间隔
    }

    // 计算平均值
    handle->calib.accel_x_offset = accel_x_sum / samples;
    handle->calib.accel_y_offset = accel_y_sum / samples;
    handle->calib.accel_z_offset = (accel_z_sum / samples)-1; // 减去1g

    handle->calib.gyro_x_offset = gyro_x_sum / samples;
    handle->calib.gyro_y_offset = gyro_y_sum / samples;
    handle->calib.gyro_z_offset = gyro_z_sum / samples;

    log_info("Calibration completed. Offsets:\r\n");
    log_info("Accel: X=%d, Y=%d, Z=%d\r\n",
             handle->calib.accel_x_offset,
             handle->calib.accel_y_offset,
             handle->calib.accel_z_offset);
    log_info("Gyro: X=%d, Y=%d, Z=%d\r\n",
             handle->calib.gyro_x_offset,
             handle->calib.gyro_y_offset,
             handle->calib.gyro_z_offset);

    return STATUS_SUCCESS;
}

status_t MPU6050_SetSleepMode(mpu6050_handle_t *handle, uint8_t sleep)
{
    uint8_t value = sleep ? 0x40 : 0x00;
    return MPU6050_WriteRegister(handle, MPU6050_REG_PWR_MGMT_1, value);
}

status_t MPU6050_ReadDeviceID(mpu6050_handle_t *handle, uint8_t *id)
{
    return MPU6050_ReadRegister(handle, MPU6050_REG_WHO_AM_I, id);
}

void CalculateAngles(float ax, float ay, float az, float *pitch, float *roll)
{
    //atan2f(ay, ax)
    *pitch = atan2f(ay, sqrtf(ax * ax + az * az)) * 180.0f / 3.1415926f;
    *roll  = atan2f(-ax, sqrtf(ay * ay + az * az)) * 180.0f / 3.1415926f;
}
